package com.example.sensors;

import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import android.hardware.SensorEvent;
import android.location.Location;
import android.util.Log;

import com.example.DatabaseConstants.MainEvents;
import com.example.priorityobserver.Observable;
import com.example.priorityobserver.Observer;

public class Sensor_Transport_Mode extends AbstractSensor implements
		Observer<Map<String, Object>> {
	public static final String WALKING = "WALKING";
	public static final String RUNNING = "RUNNING";
	public static final String STILL = "STILL";
	public static final String DRIVING = "DRIVING";
	public static final String SHAKING = "SHAKING";

	private static String mode = MainEvents.Message.STILL;
	private static long StartTime = 0;
	private String mCurrentTransportMode = MainEvents.Message.STILL;
	private Map<String, ArrayList<String>> mPossibleStatesFromSensors;
	private List<String> transportState = new ArrayList<String>();
	static final String LOG_TAG = "SAMPLINGSERVICE";
	static final int MAX_BUFFER_LEN = 40;
	static final boolean DEBUG_GENERAL = false;
	static final boolean DEBUG_SHAKING = false;
	static final boolean DEBUG_WALKING = false;
	static final boolean DEBUG_RUNNING = false;
	public static final String NAME = "TRANSPORT_MODE";

	private static String currentTransportState;
	private static String relevantTransportState;

	private boolean currentlyShaking = false;
	private int shakePeakCtr = 0;
	private int shakeCtr = 0;

	private boolean currentlyWalking = false;
	private int stepCtr = 0;

	private boolean currentlyRunning = false;
	private int runStepCtr = 0;

	private PrintWriter captureFile;

	private long sampleCounter;

	private double sampleBuffer[] = new double[MAX_BUFFER_LEN];
	private int sampleBufferPtr = 0;

	private AveragingWindow w3power = new AveragingWindow(20);
	private AveragingWindow w4power = new AveragingWindow(20);
	private AveragingWindow w5power = new AveragingWindow(5);

	private PeakWindow w3peakWindow = new PeakWindow(10);
	private PeakWindow w4peakWindow = new PeakWindow(10);
	private PeakWindow w5peakWindow = new PeakWindow(5);

	private DelayWindow w3delayWindow = new DelayWindow(10);
	private PeakWindow delayedw3peakWindow = new PeakWindow(10);

	private DelayWindow w4delayWindow = new DelayWindow(10);
	private PeakWindow delayedw4peakWindow = new PeakWindow(5);

	private DelayWindow w5delayWindow = new DelayWindow(5);
	private PeakWindow delayedw5peakWindow = new PeakWindow(3);

	private LevelKeeper w5pwdelayLevelKeeper = new LevelKeeper(60);

	private long w3peakPtr = -1L;
	private long w4peakPtr = -1L;
	private long w5peakPtr = -1L;

	private double w3[] = { -0.000489182807048, // 1
			-0.000858652010111, // 2
			-0.00113583821754, // 3
			-0.00106498662267, // 4
			-0.000358029941802, // 5
			0.00116294815188, // 6
			0.00338024348745, // 7
			0.00573592771904, // 8
			0.00724126323981, // 9
			0.00672663269903, // 10
			0.00332167463362, // 11
			-0.00298492586564, // 12
			-0.0109709108507, // 13
			-0.0182613771714, // 14
			-0.0219854832897, // 15
			-0.0198603493183, // 16
			-0.0112878078472, // 17
			0.00205783949989, // 18
			0.0165239993424, // 19
			0.0276563694057, // 20
			0.0318309886184, // 21
			0.0276563694057, // 22
			0.0165239993424, // 23
			0.00205783949989, // 24
			-0.0112878078472, // 25
			-0.0198603493183, // 26
			-0.0219854832897, // 27
			-0.0182613771714, // 28
			-0.0109709108507, // 29
			-0.00298492586564, // 30
			0.00332167463362, // 31
			0.00672663269903, // 32
			0.00724126323981, // 33
			0.00573592771904, // 34
			0.00338024348745, // 35
			0.00116294815188, // 36
			-0.000358029941802, // 37
			-0.00106498662267, // 38
			-0.00113583821754, // 39
			-0.000858652010111 // 40
	};

	private double w4[] = { -0.000978365614096, // 1
			-0.00227167643509, // 2
			-0.000716059883604, // 3
			0.0067604869749, // 4
			0.0144825264796, // 5
			0.00664334926724, // 6
			-0.0219418217014, // 7
			-0.0439709665794, // 8
			-0.0225756156944, // 9
			0.0330479986849, // 10
			0.0636619772368, // 11
			0.0330479986849, // 12
			-0.0225756156944, // 13
			-0.0439709665794, // 14
			-0.0219418217014, // 15
			0.00664334926724, // 16
			0.0144825264796, // 17
			0.0067604869749, // 18
			-0.000716059883604, // 19
			-0.00227167643509, // 20
			-0.000978365614096 // 21
	};

	private double w5[] = { -0.00195673122819, // 1
			-0.00143211976721, // 2
			0.0289650529593, // 3
			-0.0438836434028, // 4
			-0.0451512313887, // 5
			0.127323954474, // 6
			-0.0451512313887, // 7
			-0.0438836434028, // 8
			0.0289650529593, // 9
			-0.00143211976721 // 10
	};

	public Sensor_Transport_Mode(AbstractSensor gpsSensor,
			AbstractSensor accelometerSensor) {
		accelometerSensor.registerObserverEnd(this);
		gpsSensor.registerObserverEnd(this);
		mPossibleStatesFromSensors = new HashMap<String, ArrayList<String>>();
	}

	private void handleGPS(Location location) {
		if (location.hasSpeed() && location.getSpeed() > 20
				&& mCurrentTransportMode != MainEvents.Message.DRIVING) {
			List<String> transportState = new ArrayList<String>();
			transportState.add(MainEvents.Message.DRIVING);
			setmPossibleStatesFromSensors(Sensor_GPS.class.getCanonicalName(),
					transportState);
		} else {
			if (!location.hasSpeed()
					&& mCurrentTransportMode != MainEvents.Message.STILL) {
				List<String> transportState = new ArrayList<String>();
				transportState.add(MainEvents.Message.STILL);
				setmPossibleStatesFromSensors(
						Sensor_GPS.class.getCanonicalName(), transportState);
			}
		}
	}

	public void setmPossibleStatesFromSensors(String Key,
			List<String> possibilityTypes) {
		mPossibleStatesFromSensors.put(Key,
				(ArrayList<String>) possibilityTypes);
		if (mPossibleStatesFromSensors.containsKey(Sensor_GPS.class
				.getCanonicalName())
				&& (mPossibleStatesFromSensors.get(Sensor_GPS.class
						.getCanonicalName())
						.contains(MainEvents.Message.DRIVING))) {
			if (mode != MainEvents.Message.DRIVING) {
				mode = MainEvents.Message.DRIVING;
				StartTime = System.currentTimeMillis() / 1000;
				Map<String, Object> eventArgs = initEventArgs();
				eventArgs.put("eventType", "sensorInfoReceived");
				eventArgs.put("transportMode", MainEvents.Message.DRIVING);
				OnSensorChanged(eventArgs);
			}
		} else {
			if (mPossibleStatesFromSensors.containsKey(Sensor_Accelometer.class
					.getCanonicalName())
					&& (mPossibleStatesFromSensors.get(
							Sensor_Accelometer.class.getCanonicalName()).size() == 1)) {
				if (mode == MainEvents.Message.WALKING
						&& mPossibleStatesFromSensors.get(
								Sensor_Accelometer.class.getCanonicalName())
								.get(0) != MainEvents.Message.WALKING) {
					if ((System.currentTimeMillis() / 1000) - StartTime > 3) {
						mode = mPossibleStatesFromSensors.get(
								Sensor_Accelometer.class.getCanonicalName())
								.get(0);
						StartTime = System.currentTimeMillis() / 1000;
						Map<String, Object> eventArgs = initEventArgs();
						eventArgs.put("eventType", "sensorInfoReceived");
						eventArgs.put("transportMode", mode);
						OnSensorChanged(eventArgs);
					}
				}
			}
		}
	}

	public String getMode() {
		return mode;
	}

	public void disableSensor() {
		super.disableSensor();

	}

	public void enableSensor() {
		super.enableSensor();
	}

	public String getSensorName() {
		return NAME;
	}

	private void processShake(double out_w5, double w5pw) {
		boolean nowShaking = w5pw > 0.4;
		if (nowShaking != currentlyShaking) {
			currentlyShaking = nowShaking;
			// Initialize counters if shaking has just been detected
			if (currentlyShaking) { // this means it was not shaking before
				shakePeakCtr = 0;
				shakeCtr = 0;
			}
			shake(currentlyShaking, shakeCtr);
		}
		double delayedout_w5 = w5delayWindow.placeValue(out_w5);
		delayedw5peakWindow.placeValue(delayedout_w5);

		if (DEBUG_SHAKING) {
			if (captureFile != null) {
				captureFile.println("0,0,0,0,0,0," + w5pw + ",0,0,"
						+ delayedout_w5);
			}
		}

		// Run peak detection if shaking was detected
		if (currentlyShaking) {
			long peakOffset = peakDetect(delayedw5peakWindow);
			if (peakOffset >= 0L) {
				if (peakOffset != w5peakPtr) {
					Log.d(LOG_TAG, "processShake: new peak at " + peakOffset
							+ " (previous at " + w5peakPtr + ")");
					// New peak detected, step the shake counter (every second
					// peak is a new shake)
					w5peakPtr = peakOffset;
					++shakePeakCtr;
					if (shakePeakCtr >= 2) {
						shakePeakCtr = 0;
						++shakeCtr;
						shake(currentlyShaking, shakeCtr);
					}
				}
			}
		}
	}

	private void handleAccelometer(SensorEvent sensorEvent) {
		float[] values = sensorEvent.values;
		if (values.length < 3)
			return;
		++sampleCounter;
		if (sampleCounter > 0xFFFFFFF0L)
			sampleCounter = 0L;
		double ampl = Math.sqrt((double) values[0] * values[0]
				+ (double) values[1] * values[1] + (double) values[2]
				* values[2]);
		ampl -= 9.81; // subtract the Earth's gravity accel to decrease the
						// initial ripple in the filters
		sampleBuffer[sampleBufferPtr++] = ampl;
		sampleBufferPtr = sampleBufferPtr % MAX_BUFFER_LEN;
		double w3out = convolution(w3);
		double w4out = convolution(w4);
		double w5out = convolution(w5);
		w3peakWindow.placeValue(w3out);
		w4peakWindow.placeValue(w4out);
		w5peakWindow.placeValue(w5out);
		double w3pw = w3power.calculatePower(w3out);
		double w4pw = w4power.calculatePower(w4out);
		double w5pw = w5power.calculatePower(w5out);
		double delayedw5pw = w5pw;
		// double delayedw5pw = w5pwdelayWindow.placeValue( w5pw );
		double w5pwlevelkept = w5pwdelayLevelKeeper.getLevel(delayedw5pw);

		if (DEBUG_GENERAL) {
			if (captureFile != null) {
				for (int i = 0; i < values.length; ++i) {
					if (i > 0)
						captureFile.print(",");
					captureFile.print(Float.toString(values[i]));
				}
				captureFile.print("," + ampl);
				captureFile.print("," + w3out);
				captureFile.print("," + w4out);
				captureFile.print("," + w5out);
				captureFile.print("," + w3pw);
				captureFile.print("," + w4pw);
				captureFile.print("," + w5pw);
				captureFile.println();
			}
		}

		currentTransportState = "";
		// Processes the shake movement
		processShake(w5out, w5pw);
		// Processes the walking movement
		processWalking(w3out, w3pw, w4pw, w5pwlevelkept);
		// Processes the running movement
		processRunning(w4out, w3pw, w4pw, w5pwlevelkept);
		if (currentTransportState == "") {
			currentTransportState = Sensor_Transport_Mode.STILL;
		}
		if (relevantTransportState != currentTransportState) {
			relevantTransportState = currentTransportState;
			transportState = new ArrayList<String>();
			transportState.add(relevantTransportState);
			Map<String, Object> stateChangedParameters = initEventArgs();
			stateChangedParameters.put("eventType", "sensorInfoReceived");
			stateChangedParameters
					.put("transportState", relevantTransportState);
			OnSensorChanged(stateChangedParameters);
		}

	}

	public void update(Observable<Map<String, Object>> arg0,
			Map<String, Object> eventArgs) {
		if (isSensorEnabled == false) {
			return;
		}
		if (eventArgs.get("eventType").equals("sensorInfoReceived")) {
			if (arg0 instanceof Sensor_GPS) {
				if (eventArgs.containsKey("location")) {
					this.handleGPS((Location) eventArgs.get("location"));
				}
			}
			if (arg0 instanceof Sensor_Accelometer) {
				if (eventArgs.containsKey("sensorEvent")) {
					this.handleAccelometer((SensorEvent) eventArgs
							.get("sensorEvent"));
				}
			}
		}
	}

	private long peakDetect(PeakWindow peakWindow) {
		int peakOffset = 0;
		boolean peakSet = false;
		double peakValue = 0.0;
		int bufPtr = peakWindow.getWindowPtr();
		for (int i = -1; i >= -(peakWindow.getWindowLength()); --i) {
			bufPtr = peakWindow.adjustPtr(--bufPtr);
			if (!peakSet || (peakWindow.getValue(bufPtr) > peakValue)) {
				peakValue = peakWindow.getValue(bufPtr);
				peakSet = true;
				peakOffset = i;
			}
		}
		// don't accept peaks near to the beginning and end of buffer
		if ((peakOffset <= -(peakWindow.getWindowLength()))
				|| (peakOffset >= -1))
			return -1L; // no peak detected
		return sampleCounter + peakOffset;
	}

	private void shake(boolean isShaking, int count) {
		currentTransportState = Sensor_Transport_Mode.SHAKING;
	}

	private void processWalking(double out_w3, double w3pw, double w4pw,
			double w5pw) {
		boolean nowWalking = (w5pw < 0.4)
				&& (((w3pw > 0.2) && (w3pw < 0.8)) || ((w4pw > 0.2) && (w4pw < 0.4)));
		if (nowWalking != currentlyWalking) {
			currentlyWalking = nowWalking;
			if (currentlyWalking) {
				stepCtr = 0;
			}
			walk(currentlyWalking, stepCtr);
		}
		double delayedout_w3 = w3delayWindow.placeValue(out_w3);
		delayedw3peakWindow.placeValue(delayedout_w3);

		if (DEBUG_WALKING) {
			double cw = 0.0;
			if (currentlyWalking)
				cw = 1.0;
			if (captureFile != null) {
				captureFile.println("0,0,0,0," + delayedout_w3 + "," + cw
						+ ",0," + w3pw + "," + w4pw + "," + w5pw);
			}
		}

		if (currentlyWalking) {
			long peakOffset = peakDetect(delayedw3peakWindow);
			if (peakOffset >= 0L) {
				if (peakOffset != w3peakPtr) {
					Log.d(LOG_TAG, "processRunning: new peak at " + peakOffset
							+ " (previous at " + w3peakPtr + ")");
					// New peak detected, step the step counter
					w3peakPtr = peakOffset;
					++stepCtr;
					walk(currentlyWalking, stepCtr);
				}
			}
		}
	}

	private void walk(boolean isWalking, int count) {
		currentTransportState = Sensor_Transport_Mode.WALKING;
	}

	private void run(boolean isRunning, int count) {
		currentTransportState = Sensor_Transport_Mode.RUNNING;
	}

	private void processRunning(double out_w4, double w3pw, double w4pw,
			double w5pw) {
		boolean nowRunning = (w5pw < 0.7) && (w4pw > 0.4);
		if (nowRunning != currentlyRunning) {
			currentlyRunning = nowRunning;
			if (currentlyRunning) {
				runStepCtr = 0;
			}
			run(currentlyRunning, runStepCtr);
		}
		double delayedout_w4 = w4delayWindow.placeValue(out_w4);
		delayedw4peakWindow.placeValue(delayedout_w4);

		if (DEBUG_RUNNING) {
			double cw = 0.0;
			if (currentlyRunning)
				cw = 1.0;
			if (captureFile != null) {
				captureFile.println("0,0,0,0,0," + delayedout_w4 + "," + cw
						+ "," + w3pw + "," + w4pw + "," + w5pw);
			}
		}

		if (currentlyRunning) {
			long peakOffset = peakDetect(delayedw4peakWindow);
			if (peakOffset >= 0L) {
				if (peakOffset != w4peakPtr) {
					Log.d(LOG_TAG, "processRunning: new peak at " + peakOffset
							+ " (previous at " + w4peakPtr + ")");
					// New peak detected, step the step counter
					w4peakPtr = peakOffset;
					++runStepCtr;
					run(currentlyRunning, runStepCtr);
				}
			}
		}
	}

	private double convolution(double filter[]) {
		int ctr = sampleBufferPtr - 1;
		double result = 0.0;
		for (int i = 0; i < filter.length; ++i) {
			if (ctr < 0)
				ctr = MAX_BUFFER_LEN - 1;
			result += filter[i] * sampleBuffer[ctr];
			--ctr;
		}
		return result;
	}

	class PeakWindow {
		int windowLength;
		int windowPtr;
		double window[];

		public PeakWindow(int windowLength) {
			this.windowLength = windowLength;
			window = new double[windowLength];
			init();
		}

		public void init() {
			windowPtr = 0;
			for (int i = 0; i < windowLength; ++i)
				window[i] = 0.0;
		}

		public void placeValue(double value) {
			window[windowPtr++] = value;
			windowPtr = adjustPtr(windowPtr);
		}

		public int adjustPtr(int ptr) {
			if (ptr < 0)
				return windowLength - 1;
			if (ptr >= windowLength)
				return 0;
			return ptr;
		}

		public int getWindowLength() {
			return windowLength;
		}

		public int getWindowPtr() {
			return windowPtr;
		}

		public double getValue(int ptr) {
			return window[ptr];
		}
	}

	class AveragingWindow {
		int windowLength;
		int windowPtr;
		double window[];

		public AveragingWindow(int windowLength) {
			this.windowLength = windowLength;
			window = new double[windowLength];
			init();
		}

		public void init() {
			windowPtr = 0;
			for (int i = 0; i < windowLength; ++i)
				window[i] = 0.0;
		}

		public double calculatePower(double input) {
			window[windowPtr] = input * input;
			windowPtr = ++windowPtr % windowLength;
			double result = 0.0;
			for (int i = 0; i < window.length; ++i)
				result += window[i];
			result = Math.sqrt(result / windowLength);
			return result;
		}
	}

	class DelayWindow {
		int windowLength;
		int windowPtr;
		double window[];

		public DelayWindow(int windowLength) {
			this.windowLength = windowLength;
			window = new double[windowLength];
			init();
		}

		public void init() {
			windowPtr = 0;
			for (int i = 0; i < windowLength; ++i)
				window[i] = 0.0;
		}

		public double placeValue(double input) {
			double result = window[windowPtr];
			window[windowPtr] = input;
			windowPtr = ++windowPtr % windowLength;
			return result;
		}
	}

	class LevelKeeper {
		int keepLength;
		double levelKept;
		int levelKeepingCounter;

		public LevelKeeper(int keepLength) {
			this.keepLength = keepLength;
			init();
		}

		public void init() {
			levelKeepingCounter = 0;
		}

		public double getLevel(double input) {
			double result = input;
			--levelKeepingCounter;
			if ((levelKeepingCounter < 0) || (input > levelKept)) {
				levelKept = input;
				levelKeepingCounter = keepLength;
			} else {
				result = levelKept;
			}
			return result;
		}
	}
}
